#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>

#include <cstdlib>
#include <iostream>

// #include "ara/exec/execution_client.h"
// #include "ara/exec/recovery_action_client.h"
// #include "ara/exec/state_client.h"
// #include "ara/exec/function_group.h"
#include <thread>
#include <chrono>



#define MYPORT "4950"    

#define MAXBUFLEN 100

// IPv4 or IPv6:
void *get_in_addr(struct sockaddr *sa)
{
    if (sa->sa_family == AF_INET) {
        return &(((struct sockaddr_in*)sa)->sin_addr);
    }

    return &(((struct sockaddr_in6*)sa)->sin6_addr);
}

enum CarState {
    Forward, Backward, Left, Right, Start, Stop
};

int main()
{
    // ara::exec::ExecutionClient client;
    // client.ReportExecutionState(ara::exec::ExecutionState::kRunning);
    for (int i = 0; i < 10;i++) {
        // printf("this is Listener Action :%d\n",i);
    }

    int sockfd;
    struct addrinfo hints, *servinfo, *p;   
    int rv;
    int numbytes;
    struct sockaddr_storage their_addr;
    char buf[MAXBUFLEN];
    socklen_t addr_len;                   
    char s[INET6_ADDRSTRLEN];
    enum CarState state;
    while(1){
        std::cout<<"demo-process-AAListener is on" << std::endl;
		// zero.
        memset(&hints, 0, sizeof hints);
		// set udp protoal.
        hints.ai_family = AF_INET6; // or use IPv4
        hints.ai_socktype = SOCK_DGRAM;
        hints.ai_flags = AI_PASSIVE;

		// get addrinfo, assert.
        if ((rv = getaddrinfo(NULL, MYPORT, &hints, &servinfo)) != 0) {
            fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(rv));
            return 1;
        }
        for(p = servinfo; p != NULL; p = p->ai_next) {
			// create socket.
            if ((sockfd = socket(p->ai_family, p->ai_socktype,
                    p->ai_protocol)) == -1) {
                perror("socket error");
                continue;
            }
			// printf("socket success.\n");
            int optval = 1;
            setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));
            if (bind(sockfd, p->ai_addr, p->ai_addrlen) == -1) {
                close(sockfd);
                perror("bind error");
                continue;
            }
            break;
        }
        if (p == NULL) {
            fprintf(stderr, "failed to bind socket\n");
            return 2;
        }
        freeaddrinfo(servinfo);
        printf("waiting to recvfrom...\n");
        addr_len = sizeof their_addr;
        if ((numbytes = recvfrom(sockfd, buf, MAXBUFLEN-1 , 0,
            (struct sockaddr *)&their_addr, &addr_len)) == -1) {
            perror("recvfrom error");
            exit(1);
        }
        // printf("got packet from %s\n", inet_ntop(their_addr.ss_family, 
        //                                 get_in_addr((struct sockaddr *)&their_addr), s, sizeof s));
        // printf("packet is %d bytes long\n", numbytes);
        buf[numbytes] = '\0';
        // printf("packet: \"%s\"\n", buf);
        
        //pythoncall logic 2=============================
        std::string buf_str;
        buf_str.assign(buf);
        std::cout << buf_str << std::endl;
        // if(buf_str.find("forward") != std::string::npos) {
        //     state = Forward;
        //     printf("Forward!!!===============================\n"); //execute python
        // } else if (buf_str.find("backward") != std::string::npos) {
        //     state = Backward;
        //     printf("Backward!!!==============================\n");
        // } else if (buf_str.find("left") != std::string::npos) {
        //     state = Left;
        //     printf("Left!!!================================\n");
        // } else if (buf_str.find("right") != std::string::npos) {
        //     state = Right;
        //     printf("Right!!!==========================\n");
        // } else if (buf_str.find("start") != std::string::npos) {
        //     state = Start;
        //     printf("start==================\n");
        // } else if (buf_str.find("stop") != std::string::npos) {
        //     state = Stop;
        //     printf("stop==================\n");
        // } else if (buf_str.find("turn") != std::string::npos) {
        //     std::cout << "enter the loop" << std::endl;
        //     int returnCode = system("sudo python3 /home/pi/TurboPi/MecanumControl/Car_Move_Demo.py");
        //     if (returnCode == 0) {
        //         std::cout << "executed" << std::endl;
        //     }
        //     else {
        //         std::cout << "failed "
        //                 "non-zero: "
        //             << returnCode << std::endl;
        //     }
        // } 
    }
    close(sockfd);
    return 0;
}

